#include <iostream>
#include <pcl-1.8/pcl/ModelCoefficients.h>
#include <pcl-1.8/pcl/io/pcd_io.h>
#include <pcl-1.8/pcl/point_types.h>
#include <pcl-1.8/pcl/sample_consensus/method_types.h>
#include <pcl-1.8/pcl/sample_consensus/model_types.h>
#include <pcl-1.8/pcl/segmentation/sac_segmentation.h>
#include <pcl-1.8/pcl/filters/voxel_grid.h>
#include <pcl-1.8/pcl/filters/extract_indices.h>
#include "visual.h"
#include "getnormal.h"
#include "segment.h"
#include "filter.h"

int main(int argc, char** argv)
{
    std::cout<<"initializing..."<<std::endl;
    pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2), cloud_filtered_blob (new pcl::PCLPointCloud2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_p (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCDReader reader;
    reader.read ("/home/anthracene/Desktop/Anthracene/git/pcl_recognize_box/data/pointcloud/data2/45300.pcd1.pcd2.pcd3.pcd4.pcd5.pcd6.pcd7.pcd", *cloud_blob);
    std::cerr << "PointCloud before filtering: " << cloud_blob->width * cloud_blob->height << " data points." << std::endl;

    //创建一个点云图的窗口
    pcl::visualization::CloudViewer viewer("cloud viewer");

    //下采样
    pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
    sor.setInputCloud(cloud_blob);
    sor.setLeafSize (0.01f, 0.01f, 0.01f);
    sor.filter(*cloud_filtered_blob);

    pcl::fromPCLPointCloud2(*cloud_filtered_blob, *cloud_filtered);

    std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height << " data points." << std::endl;
    
    //去除距离较远的点云
    cloud_filtered = remove_remote_layer(cloud_filtered);

    //获得聚类
    pcl::PointCloud<pcl::PointXYZ>::Ptr res = get_forward_plane(cloud_filtered);

    //初始化点云显示
    viewer.showCloud(res);
    viewer.runOnVisualizationThreadOnce(viewerOneOff);
    viewer.runOnVisualizationThread(viewerPsycho);

    while(!viewer.wasStopped())
    {
    }
    return 0;
}